/**
    Copyright 2015 Table Rider
    By: Kristoffer Dahl & Richard Solti
*/

#ifndef DEFINITIONS_H_INCLUDED
#define DEFINITIONS_H_INCLUDED
//extern "C" {
  #include "../../system/CMSIS/Device/ATMEL/sam3xa/include/sam3x8e.h"
  //#include "system_sam3x.h"
//}
#include "ControllerMotor.h"
//extern ControllerMotor *control;

#include "Arduino.h"

#define REG_ADC_CDR7		(*(RoReg*) 	0x400C006C)
#define REG_ADC_CDR6		(*(RoReg*) 	0x400C0068)

//PWM Interrupt Enable Register 1
#define PWM_IER1 0x40094010
//PWM Interrupt Status Register 1
#define PWM_ISR1 0x4009401C

#define LEFT_MOTOR          1<<24 //PERIOD 4200 //PIOC 24 Arduino: 6
#define RIGHT_MOTOR         1<<23 //PERIOD 4200 //PIOC 23 Arduino: 7
#define LEFT_CHANNEL        1<<7
#define RIGHT_CHANNEL       1<<6
#define PWM_PERIOD          4200

#define LEFT_CURRENT        1<<0 //Pin A7
#define RIGHT_CURRENT       1<<1 //Pin A6

#define LEFT_DIRECTION      1<<0
#define RIGHT_DIRECTION     1<<1

#define AVE_PULSE_P_SEC 45000 //54585
#define SAMPLE_INTERVAL 0.1 //multiplied by 1 millisecond
// SenseCurrent Arduino: A6

//senseCurrent PA3 Arduino: DP1
//sensecurent2 PA4 Arduino: DP2

#define D1_trig             1 // PIOC Arduino: 33 Udoo: j8.12
#define D1_echo             4 // PIOC Arduino: 36 Udoo: j8.15

#define D2_trig             2 // PIOC Arduino: 34 Udoo: j8.13
#define D2_echo             5 // PIOC Arduino: 37 Udoo: j8.16

#define D3_trig             3 // PIOC Arduino: 35 Udoo: j8.14
#define D3_echo             6 // PIOC Arduino: 38 Udoo: j8.17

#define SLEEP1              7 // PIOC Arduino: 39 Udoo: j8.18
#define SLEEP2              8 // PIOC Arduino: 40 Udoo: j8.19

// encoder 1 - j17.3 - pb25- DP2
//             j21.6 - pb27- DP13
//
//encoder 2

inline void Init(void)
{
    //Enable EVERYTHING, I TELL YOU EVERYTHING! EEEEEEEEEEVEEEEEEEEERYTHIIIIING!!!!!!!!!!!!!!
    REG_PMC_PCER0=0xFFFFFFFF;
    REG_PMC_PCER1=0x1FFF;

    //Configure PWM for both motors
    REG_PIOC_PDR = LEFT_MOTOR | RIGHT_MOTOR; //Disable I/O function on PWM pins
    REG_PIOC_ABSR = REG_PIOC_ABSR | LEFT_MOTOR | RIGHT_MOTOR; //Enable peripheral (PWM) function on same pins
    //*REG_PWMC_CH7_CMR = 0x0; //Channel MODE
	REG_PWM_CMR7 = 0x0;
	REG_PWM_CPRD7 = PWM_PERIOD;
	REG_PWM_CDTY7 = 0x0;
	REG_PWM_CMR6 = 0x0;
	REG_PWM_CPRD6 = PWM_PERIOD;
	REG_PWM_CDTY6 = 0x0;
    //*REG_PWMC_CH7_CPRDR = PWM_PERIOD; //Period
    //*REG_PWMC_CH7_CDTYR = 0x0;//Make sure motors don't spin
    //*REG_PWMC_CH6_CMR = 0x0;
    //*REG_PWMC_CH6_CPRDR = PWM_PERIOD;
    //*REG_PWMC_CH6_CDTYR = 0x0;
    REG_PWM_ENA = LEFT_CHANNEL | RIGHT_CHANNEL; //Start PWM
    REG_PIOD_OER = 1<<0 | 1<<1;
    REG_PIOD_PER = 1<<0 | 1<<1;

    REG_PIOC_PER = 1<<SLEEP1 | 1<<SLEEP2;
    REG_PIOC_OER = 1<<SLEEP1 | 1<<SLEEP2;
    REG_PIOC_CODR = 1<<SLEEP1 | 1<<SLEEP2;

	REG_ADC_WPMR = 0x41444300;
    //Reset the ADC
    REG_ADC_CR = 1<<0;
    REG_ADC_MR |= 1<<1 | 4<<8;

    //Configure Encoders
    REG_ADC_MR |= 0x03000200;
    REG_ADC_CGR = 0x3;

    REG_ADC_CHER = 1<<6 | 1<<7;//1<<(6+left);//Enable ADC for current sensing
    //*REG_ADCC_CHER = 1<<(6+left);


    //Configure timer blocks for counting encoder signal
	REG_TC0_WPMR = 0x54494D00; // Disable Write-protect if enabled
	REG_TC2_WPMR = 0x54494D00;

    REG_TC2_BMR = 1<<8 | 1<<9;//3<<8;
    REG_TC2_CCR0 = 1<<0 | 1<<2;//5; //SWTRG, CLKEN
    REG_TC2_CMR0 = 1<<0 | 1<<2;//5; //XC0
    REG_TC0_CMR0 = 1<<0 | 1<<2; //XC0
    REG_TC0_BMR = 1<<8 | 1<<9;
    REG_TC0_CCR0 = 1<<0 | 1<<2; //SWTRG, CLKEN

    //Configure SysTick
    //*REG_SYSTICK_CTRL = 0x0;//Disable the counter
    //*REG_SYSTICK_LOAD = (int)(10500 * SAMPLE_INTERVAL); //Set load value to SysTick: 1 ms (sysReload = 10500)
    //*REG_SYSTICK_VAL = 0;//Clear the current value of SysTick counter
    //*REG_SYSTICK_CTRL = 0x3;//Enable SysTick exception request and enable the counter


    //NVIC setup
    //Disable the selected Interrupt
    NVIC_DisableIRQ((IRQn_Type)36);
    //Clearing any pending interrupt
    NVIC_ClearPendingIRQ((IRQn_Type)36);
    //Set priority of the interrupt
    NVIC_SetPriority((IRQn_Type)36,0);
    //Enable interrupt channel 7
    REG_PWM_IER1 = 1<<7;
    //Enable irq interrupt
    NVIC_EnableIRQ((IRQn_Type)36);


}

inline int bigMinusSmall(int a, int b)
{
    int temp, temp2;
    temp = (a>=b)? a : b;
    temp2 = (temp == a)? b : a;
    return temp - temp2;
}
/*
inline void delay(int us)//12mhz ? -> 12000000=1 sek
{
    for(int i = 0; i < us; i++)
    {
        asm("nop");
    }
}
*/

inline double clamp(double low, double value, double high)
{
    if(value > high) return high;
    else if(value < low) return low;
    else return value;
}

inline int clamp(int low, int value, int high)
{
    if(value > high) return high;
    else if(value < low) return low;
    else return value;
}
inline void dconvertAD()
{
    REG_ADC_CR = 1<<1;//Start conversion
    while((REG_ADC_ISR & (1<<24)) != (1<<24))//Wait until IDR_DRDY
    {
        asm("nop");
    }
}

extern long ticks;
extern int pwm_ticks;
extern ControllerMotor * control;

//void SysTick_Handler(void);
#define PPD 1 //Pulses to turn one degree
#define PPMM 1 //Pulses to move 1 mm
#endif // DEFINITIONS_H_INCLUDED
